// #include <pcl/filters/voxel_grid.h>
// #include <pcl/filters/radius_outlier_removal.h>

// #include <pcl/filters/statistical_outlier_removal.h>
// #include <pcl/point_cloud.h>
// #include <pcl/point_types.h>
// #include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Dense>

// #include <pcl/features/normal_3d.h>
// #include <pcl/keypoints/harris_3d.h>
// #include <pcl/keypoints/sift_keypoint.h>
// #include <pcl/segmentation/sac_segmentation.h>
// #include <pcl/segmentation/extract_clusters.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <vector>
#include <cmath>
typedef struct _Point2D_ {
    float x;
    float y;
    float yaw;
} Point2D;
typedef std::vector<Point2D> PointCloud2D;
typedef std::shared_ptr<PointCloud2D> PointCloud2DPtr;
// 函数用于创建PointCloud2DPtr并分配内存
inline PointCloud2DPtr CreatePointCloud2D() {
    return std::make_shared<PointCloud2D>();
}

// 通用的发布点云函数
void PublishPointCloud(const PointCloud2DPtr& cloud, ros::Publisher& pub);

// 函数用于检查两个点云间距离大于0.01米，如果大于0.01米，且小于0.1米则连续插入点云，直到点云间距小于0.01米
PointCloud2DPtr CheckAndInsertPoints(const PointCloud2DPtr& cloud, float min_distance, float max_distance);

// 根据输入的点云拟合线段，并发布所有线段的点云。
std::vector<PointCloud2DPtr> FitLines(const PointCloud2DPtr& points, ros::Publisher& line_pub, int size_threshold, float min_distance);

// 检查线段组中是否存在直角
PointCloud2DPtr CheckRightAngleInLines(const std::vector<PointCloud2DPtr>& lines, ros::Publisher& angle_pub, size_t step, float min_angle);

// 函数用于将ROS的点云消息转换为PointCloud2D格式
void RosMsgToPointCloud2D(const sensor_msgs::PointCloud2& cloud_msg, PointCloud2D& cloud);

// 函数用于将PointCloud2D转换为ROS的点云消息
void PointCloud2DToRosMsg(const PointCloud2D& cloud, sensor_msgs::PointCloud2& cloud_msg);

// 回调函数，用于处理接收到的点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, ros::Publisher& original_pub, ros::Publisher& voxel_pub, ros::Publisher& cross_pub, ros::Publisher& keypoints_pub, ros::Publisher& interpolated_pub, ros::Publisher& curve_pub);

PointCloud2DPtr PreprocessPointCloud(const PointCloud2DPtr& cloud_msg, int mean_k, double stddev_mul_thresh);

// 通用的发布点云函数
void PublishPointCloud(const PointCloud2DPtr& cloud, ros::Publisher& pub)
{
    sensor_msgs::PointCloud2 output;
    PointCloud2DToRosMsg(*cloud, output);
    output.header.frame_id = "map"; // 设置frame_id
    output.header.stamp = ros::Time::now(); // 设置时间戳
    pub.publish(output);
}


// 函数用于检查两个点云间距离大于0.01米，如果大于0.01米，且小于0.1米则连续插入点云，直到点云间距小于0.01米
PointCloud2DPtr CheckAndInsertPoints(const PointCloud2DPtr& cloud, float min_distance, float max_distance)
{
    PointCloud2DPtr cloud_with_interpolated_points = CreatePointCloud2D();
    for (size_t i = 0; i < cloud->size() - 1; ++i) {
        const auto& point1 = cloud->at(i);
        const auto& point2 = cloud->at(i + 1);
        float distance = std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2));
        if (distance > min_distance && distance < max_distance) {
            int num_insertions = static_cast<int>(distance / min_distance);
            float dx = (point2.x - point1.x) / (num_insertions + 1);
            float dy = (point2.y - point1.y) / (num_insertions + 1);
            for (int j = 1; j <= num_insertions; ++j) {
                Point2D p;
                p.x = point1.x + j * dx;
                p.y = point1.y + j * dy;
                // 其他字段可以在这里初始化，如果有的话
                p.yaw = 0.0;
                cloud_with_interpolated_points->push_back(p);
            }
        }
    }
    return cloud_with_interpolated_points;
}


/**
 * @brief 根据输入的点云拟合线段，并发布所有线段的点云。
 * 1.对points进行分组,将连续的点, 且两点间距不大于0.01, 视作线段，分为一组;
 * 2.对分组后的线段,进行筛选，如果线段长度低于10则抛弃，打印每组线段长度；
 * 
 * @param points 输入的点云，包含若干点。
 * @param line_pub 用于发布线段点云的ROS发布器。
 * @param size_threshold 线段的长度阈值，默认为100。
 * @return std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> 拟合得到的线段集合。
 */
std::vector<PointCloud2DPtr> FitLines(const PointCloud2DPtr& points, ros::Publisher& line_pub, int size_threshold, float min_distance)
{
    std::vector<PointCloud2DPtr> lines;
    PointCloud2DPtr current_line = CreatePointCloud2D();
    PointCloud2DPtr all_lines = CreatePointCloud2D(); // 用于存储所有线段的点云

    // 打印points的大小
    ROS_INFO("Total points: %lu", points->size());
    for (size_t i = 0; i < points->size() - 1; ++i) {
        const auto& point1 = points->at(i);
        const auto& point2 = points->at(i + 1);
        float distance = std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2));
        // 打印distance的大小
        // ROS_INFO("Distance: %f", distance);
        if (distance <= min_distance) { // 0.03
            current_line->push_back(point1);
        } else {
            if (current_line->size() >= size_threshold) {
                current_line->push_back(point1);
                lines.push_back(current_line);
                all_lines->insert(all_lines->end(), current_line->begin(), current_line->end()); // 将当前线段添加到所有线段的点云中
                ROS_INFO("Line segment length: %lu", current_line->size());
            }
            current_line = CreatePointCloud2D();
        }
    }
    // 添加最后一个线段
    if (current_line->size() >= size_threshold) {
        current_line->push_back(points->back());
        lines.push_back(current_line);
        all_lines->insert(all_lines->end(), current_line->begin(), current_line->end()); // 将当前线段添加到所有线段的点云中
        ROS_INFO("Line segment length: %lu", current_line->size());
    }

    // 打印lines的大小
    ROS_INFO("Total lines: %lu", lines.size());

    // 发布所有线段的点云
    PublishPointCloud(all_lines, line_pub);

    return lines;
}

float euclideanDistance(const Point2D& p1, const Point2D& p2) {
    return std::sqrt((p1.x - p2.x) * (p1.x - p2.x) +
                     (p1.y - p2.y) * (p1.y - p2.y));
}
/**
 * @brief 统计学离群点去除函数
 * 1.计算每个点到其他点的距离，并排序；
 * 2.取mean_k个最近邻点的距离，并计算平均距离和标准差；
 * 3.使用正确的条件来保留点，如果点到mean_k个最近邻点的平均距离在其标准差的倍数范围内，则保留该点；
 * 4.返回过滤后的点云。
 * 
 * @param cloud_msg 输入的点云，包含若干点。
 * @param mean_k 取mean_k个最近邻点的距离, 值越大，去除更多离群点。
 * @param stddev_mul_thresh 标准差的倍数阈值，值越大，保留越多离群点。
 * @return PointCloud2DPtr 过滤后的点云。
 */
PointCloud2DPtr PreprocessPointCloud(const PointCloud2DPtr& cloud_msg, int mean_k, double stddev_mul_thresh)
{
    PointCloud2DPtr sor_filtered_cloud = CreatePointCloud2D();

    if (cloud_msg->size() < mean_k) {
        // 如果点的数量小于mean_k，直接返回原点云
        return cloud_msg;
    }

    for (const auto& point : *cloud_msg) {  // 解引用cloud_msg
        std::vector<float> distances;
        distances.reserve(cloud_msg->size());

        // 计算当前点到其他点的距离
        for (const auto& neighbor : *cloud_msg) {  // 解引用cloud_msg
            if (&point != &neighbor) {
                float distance = euclideanDistance(point, neighbor);
                distances.push_back(distance);
                // ROS_INFO("点 (%f, %f) 到点 (%f, %f) 的距离: %f", point.x, point.y, neighbor.x, neighbor.y, distance);
            }
        }
        // 检查distances向量是否为空
        if (distances.empty()) {
            // 如果没有计算出任何距离，直接跳过该点
            continue;
        }

        // 对距离进行排序以便计算均值和标准差
        std::sort(distances.begin(), distances.end());

        // 取mean_k个最近邻点的距离
        // 确保mean_k不超过distances的实际大小
        size_t k = std::min(static_cast<size_t>(mean_k), distances.size());
        std::vector<float> nearest_distances(distances.begin(), distances.begin() + k);

        // 计算平均距离
        float sum = 0.0f;
        for (size_t i = 0; i < k; ++i) {
            sum += nearest_distances[i];
        }
        float mean = sum / k;

        // 计算标准差
        float variance = 0.0f;
        for (size_t i = 0; i < k; ++i) {
            variance += (nearest_distances[i] - mean) * (nearest_distances[i] - mean);
        }
        float stddev = std::sqrt(variance / k);

        // 使用正确的条件来保留点
        // 如果点到mean_k个最近邻点的平均距离在其标准差的倍数范围内，则保留该点
        if (mean <= stddev_mul_thresh * stddev) {
            sor_filtered_cloud->push_back(point);
        }
    }
    // 打印过滤后的点云大小
    ROS_INFO("Filtered point cloud size: %zu", sor_filtered_cloud->size());
    return sor_filtered_cloud;
}



bool isParallel(const Eigen::Vector2f& a, const Eigen::Vector2f& b, float threshold ) {
    // 计算叉乘
    float crossProduct = a.x() * b.y() - a.y() * b.x();
    // 检查叉乘结果是否接近于0
    // 打印
    ROS_INFO("crossProduct:%f", crossProduct);
    return std::abs(crossProduct) < threshold;
}

bool IsRightAngle(const PointCloud2DPtr& line, size_t i, size_t step, float min_angle)
{
    if (step < 1){
        ROS_ERROR("IsRightAngle: step %lu is too low!", step);
        step = 1;
    }
    if (i < step || i + step >= line->size()) {
        ROS_WARN("Index i is out of bounds for the given step.");
        return false;
    }
    Eigen::Vector3f p1(line->at(i - step).x, line->at(i - step).y, line->at(i - step).yaw);
    Eigen::Vector3f p2(line->at(i).x, line->at(i).y, line->at(i).yaw);
    Eigen::Vector3f p3(line->at(i + step).x, line->at(i + step).y, line->at(i + step).yaw);

    Eigen::Vector2f dir1 = {p2.x() - p1.x(), p2.y() - p1.y()};
    Eigen::Vector2f dir2 = {p3.x() - p2.x(), p3.y() - p2.y()};

    float dot_product = dir1.dot(dir2);
    float magnitude1 = dir1.norm();
    float magnitude2 = dir2.norm();
    // 防止除以零的情况
    if (magnitude1 == 0 || magnitude2 == 0) {
        ROS_WARN("magnitude1 < 0 || magnitude2 < 0!");
        return false;
    }
    float angle = std::acos(dot_product / (magnitude1 * magnitude2));
    // 计算偏差角
    float deviation_angle = angle - M_PI / 2;

    // 检查是否为直角（90度）
    if (std::abs(deviation_angle) < min_angle) {
        // 二次检测
        int second_step = step / 4;
        // 计算[i-step]到[i]的线段和[i-step/2]到[i]的线段是否接近平行
        Eigen::Vector2f half_dir1 = {p2.x() - line->at(i - second_step).x, p2.y() - line->at(i - second_step).y};
        // 计算[i+step]到[i]的线段和[i+step/2]到[i]的线段是否接近平行
        Eigen::Vector2f half_dir2 = {p2.x() - line->at(i + second_step).x, p2.y() - line->at(i + second_step).y};
        if (isParallel(dir1, half_dir1, 5e-3) && isParallel(dir2, half_dir2, 5e-3)) {
            // ROS_INFO(" Is Paraller!");
            return true;
        }
        else{
            // 打印p2的向量
            ROS_INFO("p2: (%f, %f)", p2.x(), p2.y());
            // 打印出两点的偏差角
            ROS_WARN("deviation_angle: %f", deviation_angle * 180 / M_PI);
            ROS_WARN("Not Paraller!");
        }
    }
    return false;
}
// 检查线段组中是否存在直角
PointCloud2DPtr CheckRightAngleInLines(const std::vector<PointCloud2DPtr>& lines, ros::Publisher& angle_pub, size_t step, float min_angle)
{
    // 一次扫描检测到的所有直角点
    PointCloud2DPtr right_angle_points = CreatePointCloud2D();

    // 遍历每条线段
    for (const auto& line : lines) {
        if (line->size() < step * 2 + 1) {
            ROS_WARN("Not enough points in a line to check for right angles.");
            continue;
        }

        std::vector<PointCloud2DPtr> groups;
        PointCloud2DPtr current_group(new PointCloud2D());
        bool first_point_added = false;

        // 遍历每条线段的每一点
        for (size_t i = step; i < line->size() - step; ++i) {
            // 检查是否为直角
            if (IsRightAngle(line, i, step, min_angle)) {
                ROS_INFO("Right angle found at point: (%f, %f)", line->at(i).x, line->at(i).y);
                // 创建一个新的 PointCloud2DPtr 来存储直角点
                PointCloud2DPtr angle_points = CreatePointCloud2D();
                angle_points->push_back({line->at(i - step).x, line->at(i - step).y, 0});
                angle_points->push_back({line->at(i).x, line->at(i).y, 0});
                angle_points->push_back({line->at(i + step).x, line->at(i + step).y, 0});
                // 发布检测到直角时的 p1, p2, p3, 用于查看步长是否合适
                PublishPointCloud(angle_points, angle_pub);
                // 保存直角点到向量中
                if (!first_point_added) {
                    current_group->push_back({line->at(i).x, line->at(i).y, 0});
                    first_point_added = true;
                } else {
                    Point2D prev_point = current_group->at(current_group->size() - 1);
                    Point2D curr_point = {line->at(i).x, line->at(i).y, 0};
                    float distance = sqrt(pow(curr_point.x - prev_point.x, 2) + pow(curr_point.y - prev_point.y, 2));

                    if (distance < 0.02) {
                        current_group->push_back(curr_point);
                    } else {
                        groups.push_back(current_group);
                        current_group.reset(new PointCloud2D());
                        current_group->push_back(curr_point);
                    }
                }
            }
        }

        // 添加最后一个组
        if (!current_group->empty()) {
            groups.push_back(current_group);
        }

        // 获取每个组的中间直角点
        for (const auto& group : groups) {
            if (!group->empty()) {
                size_t middle_index = group->size() / 2;
                Point2D middle_angle_point = group->at(middle_index);
                // 打印中间的直角点
                ROS_INFO("middle_angle_point: (%f,%f)", middle_angle_point.x, middle_angle_point.y);
                // 将中间的直角点添加到 right_angle_points 中
                right_angle_points->push_back(middle_angle_point);
            }
        }
    }

    if (right_angle_points->empty()) {
        ROS_INFO("No right angles found in any of the lines.");
    } else {
        ROS_INFO("Found %zu right angles.", right_angle_points->size());
    }
    ROS_INFO("---------------------------------");
    return right_angle_points;
}

void RosMsgToPointCloud2D(const sensor_msgs::PointCloud2& cloud_msg, PointCloud2D& cloud)
{
    cloud.resize(cloud_msg.data.size() / cloud_msg.point_step);
    for (size_t i = 0; i < cloud.size(); ++i) {
        const uint8_t* data = &cloud_msg.data[i * cloud_msg.point_step];
        cloud[i].x = *reinterpret_cast<const float*>(data + 0);
        cloud[i].y = *reinterpret_cast<const float*>(data + 4);
        cloud[i].yaw = *reinterpret_cast<const float*>(data + 8); // 假设yaw字段在data的第8个字节开始
    }
}

// 函数用于将PointCloud2D转换为ROS的点云消息
void PointCloud2DToRosMsg(const PointCloud2D& cloud, sensor_msgs::PointCloud2& cloud_msg)
{
    // 设置点云消息的基本信息
    cloud_msg.header.frame_id = "map"; // 根据实际情况设置frame_id
    cloud_msg.height = 1;
    cloud_msg.width = cloud.size();
    cloud_msg.is_dense = false;
    cloud_msg.point_step = 16; // 每个点有3个float字段：x, y, yaw
    cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width;
    cloud_msg.fields.resize(4);

    // 设置字段信息
    cloud_msg.fields[0].name = "x";
    cloud_msg.fields[0].offset = 0;
    cloud_msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[0].count = 1;

    cloud_msg.fields[1].name = "y";
    cloud_msg.fields[1].offset = 4;
    cloud_msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[1].count = 1;

    cloud_msg.fields[2].name = "z";
    cloud_msg.fields[2].offset = 8;
    cloud_msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[2].count = 1;

    cloud_msg.fields[3].name = "intensity";
    cloud_msg.fields[3].offset = 12;
    cloud_msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    cloud_msg.fields[3].count = 1;

    // 分配数据空间
    cloud_msg.data.resize(cloud_msg.row_step);

    // 填充数据
    for (size_t i = 0; i < cloud.size(); ++i) {
        uint8_t* data = &cloud_msg.data[i * cloud_msg.point_step];
        *reinterpret_cast<float*>(data + 0) = cloud[i].x;
        *reinterpret_cast<float*>(data + 4) = cloud[i].y;
        *reinterpret_cast<float*>(data + 8) = cloud[i].yaw;
        *reinterpret_cast<float*>(data + 12) = 1.0; // 设置intensity为1.0
    }
}

// 回调函数，用于处理接收到的点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, ros::Publisher& original_pub, ros::Publisher& voxel_pub, ros::Publisher& cross_pub, ros::Publisher& keypoints_pub, ros::Publisher& interpolated_pub, ros::Publisher& curve_pub)
{
    //发布原始数据
    original_pub.publish(cloud_msg);
    PointCloud2DPtr cloud = CreatePointCloud2D();

    // 将ROS的点云消息转换为PointCloud2D的点云格式
    RosMsgToPointCloud2D(*cloud_msg, *cloud);

    PointCloud2DPtr cloud_filtered = CreatePointCloud2D();
    cloud_filtered = PreprocessPointCloud(cloud, 100, 2.0);
    // 发布预处理后的点云
    PublishPointCloud(cloud_filtered, voxel_pub);

    PointCloud2DPtr cloud_with_interpolated_points = CreatePointCloud2D();
    cloud_with_interpolated_points = CheckAndInsertPoints(cloud_filtered, 0.01, 0.10);
    // 发布扩展后的点云
    PublishPointCloud(cloud_with_interpolated_points, interpolated_pub);

    // 直线拟合
    std::vector<PointCloud2DPtr> lines;
    lines = FitLines(cloud_with_interpolated_points, curve_pub, 30, 0.025);
    
    // 检查lines是否为空
    if (!lines.empty()) {
        // 检查直角
        PointCloud2DPtr all_right_angle_points = CreatePointCloud2D();
        all_right_angle_points = CheckRightAngleInLines(lines, keypoints_pub, 8, 0.25);

        // 设置新的目标点，其坐标是cross_point的坐标x-0.5,y-0.5
        // Point2D target_point;
        // target_point.x = cross_point.x() - 0.5;
        // target_point.y = cross_point.y() - 0.5;
        // target_point.yaw = 3.14; // 朝上
        // // 创建PointCloud2DPtr并添加目标点
        // PointCloud2DPtr target_points = CreatePointCloud2D();
        // target_points->push_back(target_point);
        // 发布目标点
        PublishPointCloud(all_right_angle_points, cross_pub);

        // 发布lines中第一条线段的点云
        // PublishPointCloud(lines[0], keypoints_pub);
    } else {
        ROS_WARN("No lines were fitted.");
    }

    ROS_INFO("----------------------------------------");
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pcl_point_cloud_reader");
    ros::NodeHandle nh;

    // 创建发布器
    ros::Publisher original_pub = nh.advertise<sensor_msgs::PointCloud2>("original_cloud", 1);
    // 创建发布器，发布插值后的点云
    ros::Publisher interpolated_pub = nh.advertise<sensor_msgs::PointCloud2>("interpolated_cloud", 1);
    ros::Publisher voxel_pub = nh.advertise<sensor_msgs::PointCloud2>("voxel_filtered_cloud", 1);
    ros::Publisher cross_pub = nh.advertise<sensor_msgs::PointCloud2>("cross_point", 1);
    ros::Publisher keypoints_pub = nh.advertise<sensor_msgs::PointCloud2>("keypoints", 1);
    ros::Publisher curve_pub = nh.advertise<sensor_msgs::PointCloud2>("fitted_line", 1);
    // 打开rosbag文件
    rosbag::Bag bag;
    bag.open("/home/zhangjj/dm_ws/src/function_test/bag/bed_scan.bag", rosbag::bagmode::Read);

    // 创建一个视图来读取bag文件中的消息
    rosbag::View view(bag, rosbag::TopicQuery("/scan_matched_points2"));

    // 播放bag文件中的消息
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::PointCloud2ConstPtr cloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
        if (cloud_msg != NULL)
        {
            pointCloudCallback(cloud_msg, original_pub, voxel_pub, cross_pub, keypoints_pub, interpolated_pub, curve_pub);
        }
        ros::Duration(0.1).sleep();
        // 检查是否有退出信号
        if (ros::isShuttingDown()) {
            ROS_INFO("收到退出信号，程序即将关闭。");
            break; // 退出for循环
        }
    }

    // 关闭bag文件
    bag.close();

    return 0;
}